module stestmod2adj
implicit none
contains

subroutine testsample1sadj(m,T,n,theta,y,test)
!lambda=1
use linear_operators
use matrixfuns
use datahadler
use polygam
integer, intent(in):: m,T,n
double precision, intent(in):: theta(m),y(T,n)
double precision, intent(out):: test(3)
integer i,j,ii
double precision, parameter:: c9=.999d0
double precision lamb(T),delta(n),c(n),phi0(n),phi1(n),phi2(n),eta,gamma(n),&
                &sigmat(n,n),gammat(n,n),alpha1,alpha2,&
				&cc(n,n),epst(1:T,1:n),isigmat(n,n),igammat(n,n)
double precision ddelta(m,n),dc(m,n),dgamma(m,n),ent(n,n*n)&
               &,dmut(m,n),dalpha1(m),dalpha2(m),alpha0,dalpha0(m),deta(m)&
			   &,dlambda(m,1),ident(n,n),wttm,fttm&
			   &,dfttm(m),dwttm(m),matwtt(m),metn(n),dphi0(m,n),dphi1(m,n),dphi2(m,n)&
			   &,jit,dcfun,grad(m),mt(n+1),vt(m+n+1,1),matV(m+n+1,m+n+1),matI(m,m)&
			   &,dvsig(m,n*n),ckc(m,n*n),ceye(m,n*n),vc(n,1),nufun&
			   &,matM(m),matIbb(n,n),matIpb(m,n),matVu(n+1,n+1),matVi(m+n+1,m+n+1)&
			   &,sbort(n),matvort(n,n),siget(n),sigeet(n)
		
ent=matent(n)
ident=eye(n)
delta=theta(1:n)
c(1)=1.0d0
c(2:n)=theta(n+1:2*n-1)
alpha0=theta(2*n)
phi0=vassign(n,theta(2*n+1))
alpha1=c9*(dsin(theta(2*n+2))**2.0d0)
alpha2=(c9-alpha1)*(dsin(theta(2*n+3))**2.0d0)

forall(i=1:n)
phi1(i)=c9*(dsin(theta(2*n+4))**2.0d0)
end forall
forall(i=1:n)
phi2(i)=(c9-phi1(i))*(dsin(theta(2*n+5))**2.0d0)
end forall
eta=theta(2*n+6)
nufun=1.0d0/eta

forall(i=1:m,j=1:n)
	ddelta(i,j)=0.0d0
	dc(i,j)=0.0d0
	dgamma(i,j)=0.0d0
	dphi0(i,j)=0.0d0
	dphi1(i,j)=0.0d0
	dphi2(i,j)=0.0d0
	dmut(i,j)=0.0d0
end forall
forall(i=1:n)
	ddelta(i,i)=1.0d0
	dphi0(2*n+1,i)=1.0d0
	dphi1(2*n+4,i)=c9*2.0d0*dsin(theta(2*n+4))*dcos(theta(2*n+4))
	dphi2(2*n+4,i)=-(c9*2.0d0*dsin(theta(2*n+4))*dcos(theta(2*n+4)))*(dsin(theta(2*n+5))**2.0d0)
	dphi2(2*n+5,i)=(c9-phi1(i))*2.0d0*dsin(theta(2*n+5))*dcos(theta(2*n+5))
end forall
forall(i=2:n)
	dc(n+i-1,i)=1.0d0
end forall

forall(i=1:m)
	dalpha0(i)=0.0d0
	dalpha1(i)=0.0d0
	dlambda(i,1)=0.0d0
	deta(i)=0.0d0
end forall
dalpha2=dalpha1
dalpha0(2*n)=1.0d0
dalpha1(2*n+2)=c9*2.0d0*dsin(theta(2*n+2))*dcos(theta(2*n+2))
dalpha2(2*n+2)=-dalpha1(2*n+2)*(dsin(theta(2*n+3))**2.0d0)
dalpha2(2*n+3)=(c9-alpha1)*2.0d0*dsin(theta(2*n+3))*dcos(theta(2*n+3))
deta(2*n+6)=1.0d0

forall(i=1:T,j=1:n)
	epst(i,j)=y(i,j)-delta(j)
end forall


forall(i=1:n,j=1:n)
	cc(i,j)=c(i)*c(j)
end forall

forall(i=1:n)
	dmut(i,i)=1.0d0
end forall

gamma=phi0/(1.0d0-phi1-phi2)

gammat=diag(gamma)
igammat=diag(1.0d0/gamma)
forall (i=1:m,j=1:n)
	dgamma(i,j)=((1.0d0-phi1(j)-phi2(j))*dphi0(i,j)-phi0(j)*(-dphi1(i,j)-dphi2(i,j)))&
	           &/(1.0d0-phi1(j)-phi2(j))**2.0d0
end forall
dlambda(:,1)=((1.0d0-alpha1-alpha2)*dalpha0-alpha0*(-dalpha1-dalpha2))/(1.0d0-alpha1-alpha2)**2.0d0

lamb(1)=alpha0/(1.0d0-alpha1-alpha2)
sigmat=lamb(1)*cc+gammat
isigmat=igammat-(((igammat.x.cc).x.igammat)&
	      &/(dot_product(c,matmul(igammat,c))+(1.0d0/lamb(1))))

jit=dot_product(epst(1,:),matmul(isigmat,epst(1,:)))


vc(:,1)=c
ckc=.t.kron(vc,vc)
ceye=matmul(dc,kron(.t.vc,dble(eye(n)))+kron(dble(eye(n)),.t.vc))

dvsig=lamb(1)*ceye+matmul(dgamma,ent)
forall(i=1:m+n+1,j=1:m+n+1)
	matVi(i,j)=0.0d0
end forall

!!
matVi(1:m-1,1:m-1)=(nufun*(n+nufun)/((nufun-2.0d0)*(n+nufun+2.0d0)))*matmul(dmut(1:m-1,:),isigmat.xt.dmut(1:m-1,:))&
&+(.5d0*(n+nufun)/(n+nufun+2.0d0))*matmul(dvsig(1:m-1,:),kron(isigmat,isigmat).xt.dvsig(1:m-1,:))&
&-(.5d0/(n+nufun+2.0d0))*matmul(dvsig(1:m-1,:).x.vec(isigmat),(.t.vec(isigmat)).xt.dvsig(1:m-1,:))

matVi(1:m-1,m)=-((n+2.0d0)*nufun*nufun/((nufun-2.0d0)*(n+nufun)*(n+nufun+2.0d0)))&
&*matmul(dvsig(1:m-1,:),vec2(isigmat))
matVi(m,1:m-1)=matVi(1:m-1,m)

matVi(m,m)=.25d0*(nufun**4.0d0)*(ppsi(1.0d0,nufun/2.0d0)-ppsi(1.0d0,(n+nufun)/2.0d0))&
&-(n*(nufun**4.0d0)*(nufun*nufun+n*(nufun-4.0d0)-8.0d0)&
&/(2.0d0*(nufun-2.0d0)**2.0d0*(n+nufun)*(n+nufun+2.0d0)))

matVi(m+1:m+n,m+1:m+n)=(2.0d0*(n+2.0d0)*eta*eta/((1.0d0-2.0d0*eta)*(1.0d0+(n+2.0d0)*eta)))*sigmat
matVi(1:m-1,m+1:m+n)=(-2.0d0*(n+2.0d0)*eta*eta/((1.0d0-2.0d0*eta)*(1.0d0+(n+2.0d0)*eta)))*dmut(1:m-1,:)
matVi(m+1:m+n,1:m-1)=.t.matVi(1:m-1,m+1:m+n)

matVi(m+n+1,m+n+1)=8.0d0*n*(n+2.0d0)*(eta**6.0d0)/((1.0d0-2.0d0*eta)**2.0d0*(1.0d0-4.0d0*eta)**2.0d0&
      &*(1.0d0+(n+2.0d0)*eta)*(1.0d0+(n-2.0d0)*eta))
matVi(1:m-1,m+n+1)=(4.0d0*(n+2.0d0)*(eta**4.0d0)/((1.0d0-2.0d0*eta)*(1.0d0-4.0d0*eta)&
        &*(1.0d0+(n+2.0d0)*eta)*(1.0d0+(n-2.0d0)*eta)))*matmul(dvsig(1:m-1,:),vec2(isigmat))
matVi(m,m+n+1)=-2.0d0*n*(n+2.0d0)*(eta**3.0d0)/((1.0d0-2.0d0*eta)**2.0d0*(1.0d0-4.0d0*eta)&
        &*(1.0d0+n*eta)*(1.0d0+(n+2.0d0)*eta))
matVi(m+n+1,1:m)=matVi(1:m,m+n+1)
matV=matVi
!!

mt(1:n)=(eta*(jit-n-2.0d0)/(1.0d0-2.0d0*eta+eta*jit))*epst(1,:)
mt(n+1)=(eta*eta/((1.0d0-2.0d0*eta)*(1.0d0-4.0d0*eta)))&
         &*((jit-n*(1.0d0-2.0d0*eta))/(1.0d0-2.0d0*eta+eta*jit))&
		 &+(eta*eta*(n-jit)/((1.0d0-2.0d0*eta)*(1.0d0+(n-2.0d0)*eta)))

forall(i=1:n)
	sigeet(i)=((n*eta+1.0d0)/(1.0d0-2.0d0*eta+eta*jit))*(dot_product(isigmat(i,:),epst(1,:))*&
	          & dot_product(isigmat(:,i),epst(1,:)))-isigmat(i,i)
end forall
siget=matmul(isigmat,epst(1,:))

call dcgive(eta)
grad=((n*eta+1.0d0)/(1.0d0-2.0d0*eta+eta*jit))*matmul(dmut,siget)&
    &+.5d0*matmul(dgamma,sigeet)&
	&+.5d0*dlambda(:,1)*(((n*eta+1.0d0)/(1.0d0-2.0d0*eta+eta*jit))*(dot_product(c,siget)**2.0)-dot_product(c,matmul(isigmat,c)))&
	&+lamb(1)*(((n*eta+1.0d0)/(1.0d0-2.0d0*eta+eta*jit))*dot_product(c,siget)*matmul(dc,siget)&
	&-matmul(matmul(dc,isigmat),c))+(dcfun+dcoefg(jit))*deta


!t>1
do i=2,T,1
	wttm=lamb(i-1)/(1.0d0+lamb(i-1)*dot_product(c,c/gamma))
	fttm=wttm*dot_product((c/gamma),epst(i-1,:))
	lamb(i)=alpha0+alpha1*((fttm**2.0d0)+wttm)+alpha2*lamb(i-1)

	forall(ii=1:n)
	    metn(ii)=dot_product(igammat(ii,:),c)*dot_product(igammat(:,ii),epst(i-1,:))
	end forall
	matwtt=matmul(matmul(dgamma,ent),vec2((igammat.x.cc).x.igammat))

	dwttm=-(wttm*wttm)*(-(lamb(i-1)**(-2.0))*dlambda(:,1)+2.0d0*matmul(dc,matmul(igammat,c))&
	      &-matwtt)
	
	dfttm=dwttm*dot_product(c,matmul(igammat,epst(i-1,:)))&
	     &+wttm*matmul(dc,matmul(igammat,epst(i-1,:)))&
		 &-wttm*matmul(dgamma,metn)-wttm*matmul(dmut,matmul(igammat,c))


	forall (ii=1:m,j=1:n)
		dgamma(ii,j)=dphi0(ii,j)+dphi1(ii,j)*(((epst(i-1,j)-c(j)*fttm)**2.0d0)&
		                                                                   &+(c(j)*c(j))*wttm)&
		           &+dphi2(ii,j)*gamma(j)&
				   &+phi1(j)*2.0d0*(epst(i-1,j)-c(j)*fttm)*&
				   &(-dmut(ii,j)-dc(ii,j)*fttm-c(j)*dfttm(ii))&
				   &+phi1(j)*2.0d0*c(j)*dc(ii,j)*wttm+phi1(j)*c(j)*c(j)*dwttm(ii)+phi2(j)*dgamma(ii,j)
	end forall


	gamma=phi0+phi1*(((epst(i-1,:)-c*fttm)**2.0d0)+(c*c)*wttm)+phi2*gamma

	gammat=diag(gamma)
	igammat=diag(1.0d0/gamma)


	sigmat=lamb(i)*cc+gammat
	isigmat=igammat-(((igammat.x.cc).x.igammat)&
	      &/(dot_product(c,matmul(igammat,c))+(1.0d0/lamb(i))))

	jit=dot_product(epst(i,:),matmul(isigmat,epst(i,:)))


	dlambda(:,1)=dalpha0+dalpha1*((fttm*fttm)+wttm)+dalpha2*(lamb(i-1))&
	      &+alpha1*(2.0d0*fttm*dfttm+dwttm)+alpha2*dlambda(:,1)

	dvsig=lamb(i)*ceye+matmul(dgamma,ent)+matmul(dlambda,ckc)

!!
matVi(1:m-1,1:m-1)=(nufun*(n+nufun)/((nufun-2.0d0)*(n+nufun+2.0d0)))*matmul(dmut(1:m-1,:),isigmat.xt.dmut(1:m-1,:))&
&+(.5d0*(n+nufun)/(n+nufun+2.0d0))*matmul(dvsig(1:m-1,:),kron(isigmat,isigmat).xt.dvsig(1:m-1,:))&
&-(.5d0/(n+nufun+2.0d0))*matmul(dvsig(1:m-1,:).x.vec(isigmat),(.t.vec(isigmat)).xt.dvsig(1:m-1,:))

matVi(1:m-1,m)=-((n+2.0d0)*nufun*nufun/((nufun-2.0d0)*(n+nufun)*(n+nufun+2.0d0)))&
&*matmul(dvsig(1:m-1,:),vec2(isigmat))
matVi(m,1:m-1)=matVi(1:m-1,m)

matVi(m,m)=.25d0*(nufun**4.0d0)*(ppsi(1.0d0,nufun/2.0d0)-ppsi(1.0d0,(n+nufun)/2.0d0))&
&-(n*(nufun**4.0d0)*(nufun*nufun+n*(nufun-4.0d0)-8.0d0)&
&/(2.0d0*(nufun-2.0d0)**2.0d0*(n+nufun)*(n+nufun+2.0d0)))

matVi(m+1:m+n,m+1:m+n)=(2.0d0*(n+2.0d0)*eta*eta/((1.0d0-2.0d0*eta)*(1.0d0+(n+2.0d0)*eta)))*sigmat
matVi(1:m-1,m+1:m+n)=(-2.0d0*(n+2.0d0)*eta*eta/((1.0d0-2.0d0*eta)*(1.0d0+(n+2.0d0)*eta)))*dmut(1:m-1,:)
matVi(m+1:m+n,1:m-1)=.t.matVi(1:m-1,m+1:m+n)

matVi(m+n+1,m+n+1)=8.0d0*n*(n+2.0d0)*(eta**6.0d0)/((1.0d0-2.0d0*eta)**2.0d0*(1.0d0-4.0d0*eta)**2.0d0&
      &*(1.0d0+(n+2.0d0)*eta)*(1.0d0+(n-2.0d0)*eta))
matVi(1:m-1,m+n+1)=(4.0d0*(n+2.0d0)*(eta**4.0d0)/((1.0d0-2.0d0*eta)*(1.0d0-4.0d0*eta)&
        &*(1.0d0+(n+2.0d0)*eta)*(1.0d0+(n-2.0d0)*eta)))*matmul(dvsig(1:m-1,:),vec2(isigmat))
matVi(m,m+n+1)=-2.0d0*n*(n+2.0d0)*(eta**3.0d0)/((1.0d0-2.0d0*eta)**2.0d0*(1.0d0-4.0d0*eta)&
        &*(1.0d0+n*eta)*(1.0d0+(n+2.0d0)*eta))
matVi(m+n+1,1:m)=matVi(1:m,m+n+1)
matV=matV+matVi
!!

mt(1:n)=mt(1:n)+((eta*(jit-n-2.0d0)/(1.0d0-2.0d0*eta+eta*jit))*epst(i,:))
mt(n+1)=mt(n+1)+((eta*eta/((1.0d0-2.0d0*eta)*(1.0d0-4.0d0*eta)))&
         &*((jit-n*(1.0d0-2.0d0*eta))/(1.0d0-2.0d0*eta+eta*jit))&
		 &+(eta*eta*(n-jit)/((1.0d0-2.0d0*eta)*(1.0d0+(n-2.0d0)*eta))))


	forall(ii=1:n)
		sigeet(ii)=((n*eta+1.0d0)/(1.0d0-2.0d0*eta+eta*jit))*(dot_product(isigmat(ii,:),epst(i,:))*&
		          &   dot_product(isigmat(:,ii),epst(i,:)))-isigmat(ii,ii)
	end forall
	siget=matmul(isigmat,epst(i,:))

	grad=grad+((n*eta+1.0d0)/(1.0d0-2.0d0*eta+eta*jit))*matmul(dmut,siget)&
	    &+.5d0*matmul(dgamma,sigeet)&
		&+.5d0*dlambda(:,1)*(((n*eta+1.0d0)/(1.0d0-2.0d0*eta+eta*jit))*(dot_product(c,siget)**2.0)-dot_product(c,matmul(isigmat,c)))&
		&+lamb(i)*(((n*eta+1.0d0)/(1.0d0-2.0d0*eta+eta*jit))*dot_product(c,siget)*matmul(dc,siget)&
		&-matmul(matmul(dc,isigmat),c))+(dcfun+dcoefg(jit))*deta

end do


matV=matV/T

matI=matV(1:m,1:m)
matIbb=matV(m+1:m+n,m+1:m+n)
matIpb=matV(1:m,m+1:m+n)
matM=matV(1:m,m+n+1)


matVu(1:n,1:n)=matIbb-matmul(matIpb.tx.(.i.matI),matIpb)
matVu(n+1,n+1)=matV(m+n+1,m+n+1)-dot_product(matM,matmul(.i.matI,matM))
matVu(1:n,n+1)=-matmul(matIpb.tx.(.i.matI),matM)
matVu(n+1,1:n)=-matmul(matIpb.tx.(.i.matI),matM)


!Ortogonalisation with respect to theta and eta hat's
mt=mt-matmul(matmul(matV(m+1:m+n+1,1:m),.i.matI),grad)


sbort=mt(1:n)-(matVu(1:n,n+1)/matVu(n+1,n+1))*mt(n+1)
matvort=matVu(1:n,1:n)-(matmul(matVu(1:n,n+1:n+1),matVu(n+1:n+1,1:n))/matVu(n+1,n+1))

!matVu(n+1,n+1)=dabs(matVu(n+1,n+1))

test(1)=(1.0d0/dsqrt(dble(T)))*mt(n+1)/dsqrt(matVu(n+1,n+1))
test(2)=(1.0d0/T)*dot_product(mt(1:n),matmul(.i.matVu(1:n,1:n),mt(1:n)))

if (mt(n+1) > 0.0d0) then
	test(3)=(1.0d0/T)*dot_product(sbort,matmul(.i.matvort,sbort))+(test(1)*test(1))
else 
	test(3)=(1.0d0/T)*dot_product(sbort,matmul(.i.matvort,sbort))
end if

contains
subroutine dcgive(eta)
double precision eta,dpsi

if (eta<8.0d-4) then
	dcfun=.25d0*n*(n+2.0d0)-2.0d0*(n*(n+2)*(n-5)/13.0d0)*eta&
	     &+3.0d0*(n*(n+2)*(n*n-6*n+16)/24.0d0)*eta*eta
else
    dcfun=(n/(2.0d0*eta*(1.0d0-2.0d0*eta)))&
	    &-(1.0d0/(2.0d0*eta**2.0d0))*(dpsi((n*eta+1.0d0)/(2.0d0*eta))-dpsi(1.0d0/(2.0d0*eta)))
end if
end subroutine dcgive

function dcoefg(ji)
double precision ji,dcoefg
    if (eta>.03d0) then
        dcoefg=-((n*eta+1.0d0)/(2.0d0*eta*(1.0d0-2.0d0*eta)))*(ji/(1.0d0-2.0d0*eta+eta*ji))&
		&+(1.0d0/(2.0d0*eta**2.0d0))*dlog(1.0d0+(eta/(1.0d0-2.0d0*eta))*ji)		
    elseif ((ji*eta)>.001d0) then
        dcoefg=-((n*eta+1.0d0)/(2.0d0*eta*(1.0d0-2.0d0*eta)))*(ji/(1.0d0-2.0d0*eta+eta*ji))&
		&+(1.0d0/(2.0d0*eta**2.0d0))*dlog(1.0d0+(eta/(1.0d0-2.0d0*eta))*ji)
    else
        dcoefg=(-.5d0*(n+2d0)*ji+.25*ji**2.0d0)&
		&+(-2.0d0*(n+2.0d0)*ji+.5d0*(n+4.0d0)*ji**2.0d0-(1.0d0/3.0d0)*ji**3.0d0)*eta&
           &+(1.0d0/6.0d0)*(-13.0d0*(n+2.0d0)*ji+6.0d0*(n+3.0d0)*ji**2.0d0-(n+6.0d0)*ji**3.0d0+(1.0d0/8.0d0)*ji**4)*3.0d0*eta**2.0d0 &
           &+(1.0/24.0d0)*(-96.0d0*(n+2.0d0)*ji+24.0d0*(8.0d0+3.0d0*n)*ji**2.0d0-24.0d0*(n+4.0d0)*ji**3.0d0+3.0d0*(n+8.0d0)*ji**4.0d0-(13.0d0/5.0d0)*ji**5.0d0)*4.0d0*eta**3.0d0 &
           &+(1.0d0/130.0d0)*(-960.0d0*(n+2.0d0)*ji+600.0d0*(2.0d0*n+5.0d0)*ji**2.0d0-1440.0d0*(3.0d0*n+10.0d0)*ji**3.0d0+130.0d0*(n+5.0d0)*ji**4.0d0-13.0d0*(n+10.0d0)*ji**5.0d0+10.0d0*ji**6.0d0)*5.0d0*eta**4.0d0
    end if
end function dcoefg
end subroutine testsample1sadj


end module stestmod2adj